#include <CType.h>
#include <string.h>
#include <asmlib.h>

int16_t strlen(int8_t *s)
{
    int16_t i = 0;

    while (*s != 0)
        i++, s++;

    return i;
}

int16_t strcmp(int8_t * s1, int8_t * s2)
{
    while ((*s1 == *s2) && *s1 != 0)
        s1++, s2++;

    return (*s1 - *s2);
}

int8_t * strcpy(int8_t * s1, int8_t * s2)
{
    char * r;
    char * ps1;
    char * ps2;

    ps1 = s1;
    ps2 = (int8_t *)s2;
    r = s1;

    while ((*ps1++ = *ps2++) != '\0')
        ;

    return (r);
}

int8_t * strncpy(int8_t * s1, int8_t * s2, size_t n)
{
    int8_t * p = s1;

    if (n != 0)
        {
        while ((*p++ = *s2++) != 0)
            {
            if (--n == 0)
                return (s1);
            }

        while (--n > 0)
            *p++ = '\0';
        }

    return (s1);
}

int16_t memcmp(void * s1, void * s2, size_t n)
{
    int8_t * p1;
    int8_t * p2;

    if (n == 0)
        return 0;

    p1 = (int8_t *)s1;
    p2 = (int8_t *)s2;

    while (*p1 != *p2)
        {
        p1++, p2++;
        if (--n == 0)
            return 0;
        }

    return (*p1 - *p2);
}

void * memcpy(void * s1, void * s2, size_t n)
{
    int8_t * p1;
    int8_t * p2;

    p1 = (int8_t *)s1;
    p2 = (int8_t *)s2;

    while (n > 0)
        {
        *p1++ = *p2++;
        n--;
        }

    return s1;
}

void * memset(void * s, int8_t c, size_t l)
{
    int8_t * p1;

    p1 = (int8_t *)s;

    while (l > 0)
        {
        *p1++ = c;
        l--;
        }

    return s;
}


int8_t * strupr(int8_t * s)
{
    int8_t * p = s;
    while (*s != '\0')
        {
        if (*s >= 'a' && *s <= 'z')
            {
            *s = *s + ('A' - 'a');
            }
        s++;
        }

    return p;
}

int8_t * strlwr(int8_t * s)
{
    int8_t * p = s;
    while (*s != '\0')
        {
        if (*s >= 'A' && *s <= 'A')
            {
            *s = *s + ('a' - 'A');
            }
        s++;
        }

    return p;
}

int8_t * strchr(int8_t * s, int16_t c)
{
    int8_t * p = s;

    while (*p != (int8_t)c)
        {
        if (*p++ == '\0')
            return NULL;
        }

    return (p);
}

int8_t * strcat(int8_t * s1, int8_t * s2)
{
    int8_t * p = s1;

    while (*s1++ != '\0')
        ;

    s1--;

    while ((*s1++ = *s2++) != '\0')
        ;

    return (p);
}

int8_t * strstr(int8_t * s1, int8_t * s2)
{
    size_t l1, l2, i;

    l1 = strlen(s1);
    l2 = strlen(s2);

    for (i = 0; i <= l1 - l2; i++)
        {
        if (memcmp(s1 + i, s2, l2) == 0)
            return s1 + i;
        }

    return NULL;
}

int8_t * strtrim(int8_t * s)
{
    int8_t * p = s;
    size_t l;

    while (*s == ' ')
        s++;

    if (p != s)
        strcpy(p, s);

    l = strlen(p);

    s = p + l - 1;

    while(*s == ' ' && s != p)
        s--;

    s++;

    *s = '\0';

    return p;
}

int8_t chrupr(int8_t c)
{
    if (c >= 'a' && c <= 'z')
        c = c + ('A' - 'a');

    return c;
}

int8_t chrlwr(int8_t c)
{
    if (c >= 'A' && c <= 'Z')
        c = c + ('a' - 'A');

    return c;
}


int8_t * itoh(uint16_t c)
{
    static int8_t buf[6];
    int8_t * map = "0123456789ABCDEF";
    int16_t i = 4;
    int16_t j;

    buf[i+1] = 0;
    while (i != 0)
        {
        buf[i--] = map[(c & 0xf)];
        c =c >> 4;
        }

    return &buf[i+1];
}

int8_t * itoa(int16_t c)
{
    static int8_t buf[10];
    int16_t f = 0;
    int16_t i = 8;

    buf[i+1] = 0;

    if (c < 0)
        f = 1, c = -c;

    if (c == 0)
        {
        buf[i--] = '0';
        }

    while (c > 0)
        {
        buf[i--] = (c % 10) + '0';
        c = c / 10;
        }

    if (f == 1)
        {
        buf[i--] = '-';
        }

    return &buf[i+1];
}

int8_t * ltoh(uint32_t c)
{
    static int8_t buf[10];
    int8_t * map = "0123456789ABCDEF";
    int16_t i = 8;
    int16_t j;

    buf[i+1] = 0;
    while (i != 0)
        {
        buf[i--] = map[(c & 0xf)];
        c = lushr(c, 4);
        }

    return &buf[i+1];
}


int8_t * ltoa(int32_t c)
{
    static int8_t buf[16];
    int32_t rad = 10;
    int16_t f = 0;
    int16_t i = 14;
    div_t r;
    int8_t *p;

    buf[i+1] = 0;

    if (c < 0)
        f = 1, c = -c;

    if (c == 0)
        {
        buf[i--] = '0';
        }

    while (c > 0)
        {
        ldiv(c, rad, &r);
        buf[i--] = r.rem + '0';
        c = r.quot;
        }

    if (f == 1)
        {
        buf[i--] = '-';
        }
    p = &buf[i+1];

    return p;
}

int16_t atoi(int8_t * s)
{
    uint16_t t = 0;
    int16_t f = 0, r = 0;

    while(*s != 0 && *s == ' ')
        {
        s++;
        }

    if (*s == '-')
        f = 1, s++;


    while(*s != 0 && *s >= '0' && *s <= '9')
        {
        t = t + *s - '0';

        if (t * 10 < t || t * 10 > MAX_INT16)
            break;

        t = t * 10;
        s++;
        }

    r = t;

    if (f == 1)
        r = -r;

    return r;
}

int32_t atol(int8_t * s)
{
    uint32_t t = 0;
    int16_t f = 0;
    int32_t r = 0;
    mul_t mul;

    while(*s != 0 && *s == ' ')
        {
        s++;
        }

    if (*s == '-')
        f = 1, s++;


    while(*s != 0 && *s >= '0' && *s <= '9')
        {
        t = t + *s - '0';

        lmul(t, 10, &mul);

        if (mul.low < t || mul.low > 0x7FFFFFFF)
            break;

        t = mul.low;

        s++;
        }

    r = t;

    if (f == 1)
        r = -r;

    return r;
}

int16_t htoi(int8_t * s)
{
    int16_t r = 0, i = 0;
    int8_t c;
    int8_t * map = "0123456789ABCDEF";
    int8_t * p;

    while(*s != 0 && (p = strchr(map, chrupr(*s))) != NULL && i < 4)
        {
        r = r << 4;
        r = r | (p - map);
        i++;
        }

    return r;
}

int32_t htol(int8_t * s)
{
    int32_t r = 0, i = 0;
    int8_t c;
    int8_t * map = "0123456789ABCDEF";
    int8_t * p;

    while(*s != 0 && (p = strchr(map, chrupr(*s))) != NULL && i < 8)
        {
        r = lushl(r, 4);
        r = r | (p - map);
        i++;
        }

    return r;
}


int16_t bcd2int(uint16_t b)
{
    int16_t r = 0;

    r = (b & 0xF) + ((b >> 4) & 0xF) * 10
        + ((b >> 8) & 0xF) * 100  + ((b >> 12) & 0xF) * 1000;

    return r;
}
